/*
 * At.c
 *
 *  Created on: Feb 19, 2024
 *      Author: hzy
 */
#include "debug.h"
#include "Global.h"
#include "At.h"
#include "Operator.h"
//#include "Flash.h"

#define BYTE_ID_CMD 1

#define CMD_FORWARD 1
#define CMD_BACKWARD 2
#define CMD_TURN_LEFT 3
#define CMD_TURN_RIGHT 4
#define CMD_TURN_AROUND 5
#define CMD_STOP 6

u8 RxIndex = 0;
u8 UARTBuff[BUFFER_SIZE];

int BuffIn = 0;
int BuffOut = 0;
int Counter = 0;

u8 Packet[PACKET_SIZE];
u8 DataString[STRING_SIZE];

u8 atMode = 1;
u8 tmMode = 0;

SysTick_Type SysTickBak;

void AtUSARTInit(void) {
    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
    SystemCoreClockUpdate();

    GPIO_InitTypeDef GPIO_InitStructure = { 0 };
    USART_InitTypeDef USART_InitStructure = { 0 };

    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOD | RCC_APB2Periph_USART1, ENABLE);

    /* USART1 TX-->D.5   RX-->D.6 */
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
    GPIO_Init(GPIOD, &GPIO_InitStructure);
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
    GPIO_Init(GPIOD, &GPIO_InitStructure);

    USART_InitStructure.USART_BaudRate = 115200;
    USART_InitStructure.USART_WordLength = USART_WordLength_8b;
    USART_InitStructure.USART_StopBits = USART_StopBits_1;
    USART_InitStructure.USART_Parity = USART_Parity_No;
    USART_InitStructure.USART_HardwareFlowControl =
    USART_HardwareFlowControl_None;
    USART_InitStructure.USART_Mode = USART_Mode_Tx | USART_Mode_Rx;

    USART_Init(USART1, &USART_InitStructure);
    USART_Cmd(USART1, ENABLE);
}

void AtTimerInit() {
    SysTickBak = *SysTick;

    NVIC_EnableIRQ(SysTicK_IRQn);
    SysTick->SR &= ~(1 << 0);
    SysTick->CMP = 20 * SystemCoreClock - 1;
    SysTick->CNT = 0;
    SysTick->CTLR = 0xF;
    //SysTick->CTLR = 0xB;
}

void AtCfg() {
    AtUSARTInit();
    //AtTimerInit();
}

int USARTRecv() {
    int ret = 0;

    while(USART_GetFlagStatus(USART1, USART_FLAG_RXNE) == SET)
    {
        UARTBuff[ BuffIn ] = USART_ReceiveData(USART1);
        BuffIn ++;
        Counter ++;

        if(BuffIn >= BUFFER_SIZE)
        BuffIn = 0;

        ret = 1;
    }

    return ret;
}

int GetByte(unsigned char* byte) {

    if (Counter <= 0)
        return 0;

    *byte = UARTBuff[BuffOut];
    BuffOut++;
    Counter--;

    if (BuffOut >= BUFFER_SIZE)
        BuffOut = 0;

    return 1;
}

int GetPacket(unsigned char* packet) {
    int startIdx = 0;
    int endIdx = 0;
    int ret = 0;
    int i = 0;
    unsigned char tmp;

    if (Counter < PACKET_SIZE)
        return ret;

    startIdx = BuffOut;
    endIdx = startIdx + PACKET_SIZE - 1;
    if (endIdx >= BUFFER_SIZE)
        endIdx = endIdx - BUFFER_SIZE;

    while( Counter >= PACKET_SIZE )
    {
        if( UARTBuff[startIdx] == 0x7E && UARTBuff[endIdx] == 0xAA )
        {
            for(i=0;i< PACKET_SIZE; i++)
            GetByte(packet + i);
            ret = 1;
            return ret;
        }
        else
        {
            GetByte(&tmp);

            if( Counter < PACKET_SIZE )
            return ret;

            startIdx = BuffOut;
            endIdx = startIdx + PACKET_SIZE - 1;
            if(endIdx >= BUFFER_SIZE)
            endIdx = endIdx - BUFFER_SIZE;
        }
    }

    return ret;
}

void SendPacket(unsigned char* packet) {
    //Send Packet Back
    int i = 0;
    while(i<7)
    {
        if(USART_GetFlagStatus(USART1, USART_FLAG_TXE) == SET)
        {
            USART_SendData(USART1,packet[i]);
            i++;
        }
    }
}

void USARTProc() {
    int ret = 0;
    uint16_t* ptrShort = 0;

    if (Counter >= 7) {
        ret = GetPacket(Packet);

        if (ret == 1) {

            // Forward
            if (Packet[BYTE_ID_CMD] == CMD_FORWARD)
            {
                OperatorForward();
                Packet[BYTE_ID_CMD] = 129;
                SendPacket(Packet);
            }
            // Backward
            else if (Packet[BYTE_ID_CMD] == CMD_BACKWARD)
            {
                OperatorBackward();
                Packet[BYTE_ID_CMD] = 130;
                SendPacket(Packet);
            }
            // Turn Left
            else if (Packet[BYTE_ID_CMD] == CMD_TURN_LEFT)
            {
                OperatorTurnLeft();
                Packet[BYTE_ID_CMD] = 131;
                SendPacket(Packet);
            }
            // Turn Right
            else if (Packet[BYTE_ID_CMD] == CMD_TURN_RIGHT)
            {
                OperatorTurnRight();
                Packet[BYTE_ID_CMD] = 132;
                SendPacket(Packet);
            }
            // Turn Around
            else if (Packet[BYTE_ID_CMD] == CMD_TURN_AROUND)
            {
                OperatorTurnAround();
                Packet[BYTE_ID_CMD] = 133;
                SendPacket(Packet);
            }
            // Stop
            else if (Packet[BYTE_ID_CMD] == CMD_STOP)
            {
                OperatorStop();
                Packet[BYTE_ID_CMD] = 134;
                SendPacket(Packet);
            }
            else {
                Packet[BYTE_ID_CMD] = 191;
                ptrShort = (uint16_t*) (&Packet[4]);
                *ptrShort = ERROR_COMMAND_ID_INVALID;
                SendPacket(Packet);
            }
        }
    }
}

void AtProc() {
    while(tmMode || atMode )
    {
        USARTRecv();

        USARTProc();
    }
}

void SysTick_Handler(void) __attribute__((interrupt("WCH-Interrupt-fast")));

/*********************************************************************
 * @fn      ADC1_IRQHandler
 *
 * @brief   ADC1_2 Interrupt Service Function.
 *
 * @return  none
 */
void SysTick_Handler(void) {
    tmMode = 0;
    SysTick->SR &= ~(1 << 0);
    NVIC_DisableIRQ(SysTicK_IRQn);

    *SysTick = SysTickBak;
}
